#include "AgentGroup.h"
#include "Agent.h"

namespace Engine
{
	void AgentGroup::AddAgent(Agent* a)
	{
		m_agents[a ->GetGameObjectID()] = a;
	}
	
	void AgentGroup::RemoveAgent(Agent* a)
	{
		auto it = m_agents.find(a ->GetGameObjectID());
		if(it != m_agents.end())
			m_agents.erase(it);
	}
	
	void AgentGroup::GetNeighboursInRange(Agent* agent, float radiusSqr, Agents* neighbours) const
	{
		for(auto it = m_agents.begin(); it != m_agents.end(); ++it)
		{
			if(it ->first == agent ->GetGameObjectID()) continue;

			if((agent ->GetPos() - it ->second ->GetPos()).SquaredLength() < radiusSqr)
				neighbours ->push_back(it ->second);
		}
	}

	Agent* AgentGroup::GetNearestNeighbour(Agent* agent)
	{
		unsigned int agentID = agent ->GetGameObjectID();
		auto closest = m_agents.begin();
		auto it = m_agents.begin();

		float closestSqrDist = std::numeric_limits<float>::max();

		for(; it != m_agents.end(); ++it)
		{
			if(it ->second ->GetGameObjectID() == agentID) continue;

			float sqrDist = (it ->second ->GetPos() - agent ->GetPos()).SquaredLength();
			if(sqrDist < closestSqrDist)
			{
				closest = it;
				closestSqrDist = sqrDist;
			}
		}

		return closest ->second;
	}
}